
#define quat	vec4
#define PI		3.14


quat Normalize(in quat q)
{
	quat	ret = q;
	float	n = q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w;
	
	if ( n == 1.0 )
		return ret;
	
	return ret / sqrt(n);
}


quat QuatFromMat3(in mat3 m)
{
	// m - need transpose?

	float	s		= 0.0;
	float	trace	= m[0][0] + m[1][1] + m[2][2];
	quat	q(0.0);
	
	if ( trace > 0.0 )
	{
		s	= sqrt( trace + 1.0 );
		q.w	= s * 0.5;
		s	= 0.5 / s;
		q.x	= ( m[1][2] - m[2][1] ) * s;
		q.y	= ( m[2][0] - m[0][2] ) * s;
		q.z	= ( m[0][1] - m[1][0] ) * s;
	}
	else
	{
		const int	nxt[3] = int[]( 1, 2, 0 );
		int			i = 0,
					j = 0,
					k = 0;
					
		if ( m[1][1] > m[0][0] )
			i = 1;
			
		if ( m[2][2] > m[1][1] )
			i = 2;
			
		j = nxt[i];
		k = nxt[j];
		s = sqrt( (m[i][i] - (m[j][j] + m[k][k])) + 1.0 );
		
		q[i] = s * 0.5;
		s	 = 0.5 / s;
		q[3] = ( m[j][k] - m[k][j] ) * s;
		q[j] = ( m[i][j] + m[j][i] ) * s;
		q[k] = ( m[i][k] + m[k][i] ) * s;
	}
	
	return Normalize( q );
}


quat Inverse(in quat q)
{
	return quat( -q.x, -q.y, -q.z, q.w );
}

/*
quat QMult(in quat left, in quat right)
{
	quat	ret;
	ret.w = (right.w * left.w) - (right.x * left.w) - (right.y * left.y) - (right.z * left.z);
	ret.x = (right.w * left.x) + (right.x * left.w) + (right.y * left.z) - (right.z * left.y);
	ret.y = (right.w * left.y) + (right.y * left.w) + (right.z * left.x) - (right.x * left.z);
	ret.z = (right.w * left.z) + (right.z * left.w) + (right.x * left.y) - (right.y * left.x);
	return ret;
}
*/

quat QMult(in quat left, in quat right)
{
	vec3	im = left.w * right.xyz + left.xyz * right.w + cross ( left.xyz, right.xyz );
	vec4	dt = left * right;
	float	re = dot ( dt, vec4 ( -1.0, -1.0, -1.0, 1.0 ) );
	return quat( im, re );
}


vec3 RotateVec(in quat qleft, in vec3 vright)
{
	vec3	uv, uuv, qvec( qleft );
	
	uv	= cross( qvec, vright );
	uuv	= cross( qvec, uv );
	uv	*= 2.0 * qleft.w;
	uuv	*= 2.0;
	
	return vright + uv + uuv;
}


vec3 RotateVec(in vec3 vleft, in quat qright)
{
	return RotateVec( qright, vleft );
}


float QDot(in quat left, in quat right)
{
	return (left.x * right.x) + (left.y * right.y) + (left.z * right.z) * (left.w * right.w);
}


quat QuatFromEuler(in vec3 vAngles)
{
	float	ang = vAngles.x * 0.5;
	float	sr	= sin( ang ),
			cr	= cos( ang );
			
	ang = vAngles.y * 0.5;
	
	float	sp	= sin( ang ),
			cp	= cos( ang );
			
	ang = vAngles.z * 0.5;
	
	float	sy	= sin( ang ),
			cy	= cos( ang );
			
	float	cpcy = cp * cy,
			spcy = sp * cy,
			cpsy = cp * sy,
			spsy = sp * sy;
			
	quat	ret;
	
	ret.x = sr * cpcy - cr * spsy;
	ret.y = cr * spcy + sr * cpsy;
	ret.z = cr * cpsy - sr * spcy;
	ret.w = cr * cpcy + sr * spsy;
	
	return ret;
}


vec3 QDirection(in quat q)
{
	return vec3( ( 2.0 * q.x * q.z + 2.0 * q.y * q.w ),
				 ( 2.0 * q.z * q.y - 2.0 * q.x * q.w ),
				 ( 1.0 - 2.0 * q.x * q.x - 2.0 * q.y * q.y ) );
}


mat3 GetQuatMatrix(in quat q)
{
	mat3	ret;
	
	ret[0]  = ( 1.0 - 2.0 * q.y * q.y - 2.0 * q.z * q.z );
	ret[3]  = ( 2.0 * q.x * q.y + 2.0 * q.z * q.w );
	ret[6]  = ( 2.0 * q.x * q.z - 2.0 * q.y * q.w );

	ret[1]  = ( 2.0 * q.x * q.y - 2.0 * q.z * q.w );
	ret[4]  = ( 1.0 - 2.0 * q.x * q.x - 2.0 * q.z * q.z );
	ret[7]  = ( 2.0 * q.z * q.y + 2.0 * q.x * q.w );

	ret[2]  = ( 2.0 * q.x * q.z + 2.0 * q.y * q.w );
	ret[5]  = ( 2.0 * q.z * q.y - 2.0 * q.x * q.w );
	ret[8]  = ( 1.0 - 2.0 * q.x * q.x - 2.0 * q.y * q.y );
	
	return ret;
}


mat4 GetQuatMatrix(in quat q, in vec3 pos = vec3(0.0))
{
	mat4	ret;

	ret[0]  = ( 1.0 - 2.0 * y * y - 2.0 * z * z );
	ret[4]  = ( 2.0 * x * y + 2.0 * z * w );
	ret[8]  = ( 2.0 * x * z - 2.0 * y * w );
	ret[12] = 0.0;

	ret[1]  = ( 2.0 * x * y - 2.0 * z * w );
	ret[5]  = ( 1.0 - 2.0 * x * x - 2.0 * z * z );
	ret[9]  = ( 2.0 * z * y + 2.0 * x * w );
	ret[13] = 0.0;

	ret[2]  = ( 2.0 * x * z + 2.0 * y * w );
	ret[6]  = ( 2.0 * z * y - 2.0 * x * w );
	ret[10] = ( 1.0 - 2.0 * x * x - 2.0 * y * y );
	ret[14] = 0.0;

	ret[3]  = pos.x;
	ret[7]  = pos.y;
	ret[11] = pos.z;
	ret[15] = 1.0;

	return ret;
}


quat QSlerp(quat q1, quat q2, float k)
{
	float	angle = dot( q1, q2 );
	
	if ( angle < 0.0 )
	{
		q1	  *= -1.0;
		angle *= -1.0;
	}
	
	float	scale,
			inv_scale;
			
	if ( (angle + 1.0) > 0.05 )
	{
		if ( (1.0 - angle) >= 0.05 )
		{
			float	theta		= acos( angle );
			float	invsintheta	= 1.0 / sin( theta );
			scale				= sin( theta * (1.0 - k) ) * invsintheta;
			inv_scale			= sin( theta * k ) * invsintheta;
		}
		else
		{
			scale	 = 1.0 - k;
			invscale = k;
		}
	}
	else
	{
		q2 = quat( -q1.y, q1.x, -q1.w, q1.z );
		scale	  = sin( PI * ( 0.5 - k ) );
		inv_scale = sin( PI * k );
	}
	
	return ( q1 * scale ) + ( q2 * inv_scale );
}


quat QuatFormAngleAxis(float fAngle, in vec3 vAxis)
{
	float	f_half_ang	= 0.5 * fAngle;
	float	f_sin		= sin( f_half_ang );
	quat	ret;
	
	ret.w = cos( f_half_ang );
	ret.x = f_sin * vAxis.x;
	ret.y = f_sin * vAxis.y;
	ret.z = f_sin * vAxis.z;
	
	return ret;
}


void QuatToAngleAxis(in quat q, out float fAngle, out vec3 vAxis)
{
	float	scale = sqrt( q.x*q.x + q.y*q.y + q.z*q.z );
	
	if ( scale == 0.0 || q.w > 1.0 || q.w < -1.0 )
	{
		fAngle	= 0.0;
		vAxis	= vec3( 0.0, 1.0, 0.0 );
	}
	else
	{
		float	inv_scale = 1.0 / scale;
		fAngle	= 2.0 * acos( q.w );
		vAxis.x	= q.x * inv_scale;
		vAxis.y	= q.y * inv_scale;
		vAxis.z	= q.z * inv_scale;
	}
}


void QuatToEuler(in quat q, out vec3 vEuler)
{
	float	sqw = q.w * q.w,
			sqx = q.x * q.x,
			sqy = q.y * q.y,
			sqz = q.z * q.z;
			
	vEuler.x = atan2( 2.0 * (q.y*q.z + q.x*q.w), -sqx - sqy + sqz + sqw );
	vEuler.y = asin( clamp( -2.0 * (q.x*q.z - q.y*q.w), -1.0, 1.0 ) );
	vEuler.z = atan2( 2.0 * (q.x*q.y + q.z*q.w), sqx - sqy - sqz + sqw );
}


quat QuatIdentity()
{
	return quat( 1.0, 0.0, 0.0, 0.0 );
}


quat QuatRotationFromTo(in quat q, in vec3 vFrom, in vec3 vTo)
{
	vec3	v0	= normalize( vFrom ),
			v1	= normalize( vTo );
	float	d	= dot( v0, v1 );
	
	if ( d >= 1.0 )
		return QuatIdentity();
	else
	
	if ( d <= -1.0 )
	{
		vec3	axis = cross( vec3( 1.0, 0.0, 0.0 ), vec3(q) );
		
		if ( length( axis ) == 0.0 )
			axis = cross( vec3( 0.0, 1.0, 0.0 ), vec3(q) );
		
		return QuatFormAngleAxis( PI, axis );
	}
	
	float	s		= sqrt( (1+d)*2 );
	float	inv_s	= 1.0 / s;
	vec3	c		= cross( v0, v1 ) * inv_s;
	
	return quat( c.x, c.y, c.z, s*0.5 );
}


quat QuatRotationX(float fAngle)
{
	float	ang = fAngle * 0.5;
	return quat( sin( ang ), 0.0, 0.0, cos( ang ) );
}


quat QuatRotationY(float fAngle)
{
	float	ang = fAngle * 0.5;
	return quat( 0.0, sin( ang ), 0.0, cos( ang ) );
}


quat QuatRotationZ(float fAngle)
{
	float	ang = fAngle * 0.5;
	return quat( 0.0, 0.0, sin( ang ), cos( ang ) );
}


quat QuatComputeW(in vec3 vQuat)
{
	float	t = 1.0 - q.x*q.x - q.y*q.y - q.z*q.z;
	float	w;
	
	if ( t < 0.0 )
		w = 0.0;
	else
		w = -sqrt( t );
		
	return quat( vQuat, w );
}
